/* usb_debug.c -- remote usb debug protocol
**
** Copyright 2006, Brian Swetland.  All rights reserved.
** See provided LICENSE file or http://frotz.net/LICENSE for details.
*/

#include "devices.h"
#include "usb.h"
#include "../tools/protocol.h"

#if 0
#define TRACE(x...) dprintf(x)
#else
#define TRACE(x...)
#endif

typedef void (*read_fn)(unsigned src_addr, void *dst, unsigned count);
typedef void (*write_fn)(void *src, unsigned dst_addr, unsigned count);

enum {
    STATE_IDLE,
    STATE_WRITING,
    STATE_READING,
};

#if WITH_JTAG
static char version[16] = "JTAG_ARM7TDMI___";
#else
static char version[16] = "LOCAL_AT91SAM7S_";
#endif

static unsigned debug_state = STATE_IDLE;
static unsigned debug_addr = 0;
static unsigned debug_count = 0;
read_fn debug_read = 0;
write_fn debug_write = 0;

static unsigned stopped = 0;
static unsigned registers[17];

void jump_to_addr(unsigned addr);
void do_nothing(void);

void read_svc_regs(unsigned *r);
void write_svc_regs(unsigned *r);

#if WITH_JTAG
#include "jtag.h"

static void debug_stop(void) {}

static void debug_jump(unsigned addr) {}

static void
jtag_read_bytes(unsigned src_addr, unsigned char *dst, unsigned count) {}

static void
jtag_write_bytes(unsigned char *src, unsigned dst_addr, unsigned count) {}

static void
jtag_read_words(unsigned src_addr, unsigned *dst, unsigned count)
{
    jtag_reset();
    jtag_halt_cpu(0);
    jtag_select_debug();
    count /= 4;

    while(count-- > 0) {
        *dst++ = jtag_mem_rd(src_addr);
        src_addr += 4;
    }
}

static void
jtag_write_words(unsigned *src, unsigned dst_addr, unsigned count)
{
    jtag_reset();
    jtag_halt_cpu(0);
    jtag_select_debug();
    count /= 4;

    while(count-- > 0) {
        jtag_mem_wr(dst_addr, *src++);
        dst_addr += 4;
    }
}

#else

static void debug_stop(void)
{
    if(!stopped) {
        read_svc_regs(registers);
        registers[15] -= 4;
        stopped = 1;
        jump_to_addr((unsigned) do_nothing);
    }
}

static void debug_jump(unsigned addr)
{
    stopped = 0;
    jump_to_addr(addr);
}

#endif

static inline void
mem_read_bytes(unsigned src_addr, unsigned char *dst, unsigned count)
{
    unsigned char *src = (unsigned char*) src_addr;
    while(count-- > 0) {
        *dst++ = *src++;
    }
}

static inline void
mem_write_bytes(unsigned char *src, unsigned dst_addr, unsigned count)
{
    unsigned char *dst = (unsigned char*) dst_addr;
    while(count-- > 0){
        *dst++ = *src++;
    }
}

static inline void
mem_read_words(unsigned src_addr, unsigned *dst, unsigned count)
{
    unsigned *src = (unsigned *) src_addr;
    count /= 4;
    while(count-- > 0) {
        *dst++ = *src++;
    }
}

static inline void
mem_write_words(unsigned *src, unsigned dst_addr, unsigned count)
{
    unsigned *dst = (unsigned*) dst_addr;
    count /= 4;
    while(count-- > 0) {
        *dst++ = *src++;
    }
}

void usb_status(unsigned online)
{
}

#if WITH_JTAG
read_fn debug_read_bytes = (read_fn) jtag_read_bytes;
read_fn debug_read_words = (read_fn) jtag_read_words;
write_fn debug_write_bytes = (write_fn) jtag_write_bytes;
write_fn debug_write_words = (write_fn) jtag_write_words;
#else
read_fn debug_read_bytes = (read_fn) mem_read_bytes;
read_fn debug_read_words = (read_fn) mem_read_words;
write_fn debug_write_bytes = (write_fn) mem_write_bytes;
write_fn debug_write_words = (write_fn) mem_write_words;
#endif

unsigned usb_bulk_recv(void *data, unsigned len)
{
    usb_msg *msg;
    
    switch(debug_state){
    case STATE_READING:
            /* if we're sending data to host and
               the host interrupts us with a new command,
               just drop it all on the floor */
        debug_state = STATE_IDLE;
        debug_count = 0;
        
    case STATE_IDLE:
        if(len < sizeof(usb_msg)) return 0;
        msg = (usb_msg*) data;

#if 0
        TRACE("debug msg { %b %b %h %x }\n",
                msg->cmd, msg->tag, msg->len, msg->addr);
#endif
        
        switch(msg->cmd) {
        case MSG_WR_WORDS:
            debug_state = STATE_WRITING;
            debug_write = debug_write_words;
            break;
        case MSG_WR_BYTES:
            debug_write = debug_write_bytes;
            debug_state = STATE_WRITING;
            break;
        case MSG_RD_REGS:
            if(!stopped) {
                read_svc_regs(registers);
                registers[15] -= 4;
            }
            msg->addr += (unsigned) registers;
            debug_read = (read_fn) mem_read_words;
            debug_state = STATE_READING;
            break;
        case MSG_RD_WORDS:
            if(msg->addr == 0xEE000000) {
                msg->addr = (unsigned) version;
                debug_read = (read_fn) mem_read_words;
            } else {
                debug_read = debug_read_words;
            }
            debug_state = STATE_READING;
            break;
        case MSG_RD_BYTES:
            debug_read = debug_read_bytes;
            debug_state = STATE_READING;
            break;
        case MSG_STOP:
            debug_stop();
            return 0;
        case MSG_RESUME:
            debug_jump(registers[15]);
            return 0;
        case MSG_JUMP:
            debug_jump(msg->addr);
            return 0;
        default:
            return 0;
        }
        
        debug_count = msg->len;
        debug_addr = msg->addr;
        return 1;

    case STATE_WRITING: 
        if(len > debug_count) len = debug_count;

        TRACE("write @ %x (%d)\n", debug_addr, len);
        debug_write(data, debug_addr, len);
        
        debug_count -= len;
        debug_addr += len;
        if(debug_count == 0) debug_state = STATE_IDLE;
        return debug_count;

    default:
        debug_state = STATE_IDLE;
        return 0;
    }
}


unsigned usb_bulk_send(void *data)
{
    unsigned len;
    
    switch(debug_state){
    case STATE_READING: 
        len  = debug_count > 64 ? 64 : debug_count;
        
        TRACE("read word @ %x (%d)\n", debug_addr, len);
        debug_read(debug_addr, data, len);
        
        debug_addr += len;
        debug_count -= len;
        if(debug_count == 0) debug_state = STATE_IDLE;
        return len;

    default:
//        TRACE("weird... no data to send, state = %d\n", debug_state);
        return 0;
    }
}

